#include "WorldGrid.h"

namespace coolpool
{
	WorldGrid::WorldGrid(int width, int length, double cell_size, EntitiesContainer * entities_container)
	{
		bottom_left_[0] = 0.0;
		bottom_left_[1] = 0.0;
		cell_size_ = cell_size;
		entities_container_ = entities_container;
		//TODO: Switched
		length_ = width;
		width_ = length;
		init();
	}

	// Initialisation of an empty grid.
	void WorldGrid::init()
	{
		WorldGridRow row(width_);
		for (int i = 0; i < length_; ++i)
		{
			world_grid_.push_back(row);
		}
	}

	WorldGrid::~WorldGrid()
	{

	}

	void WorldGrid::buildWorldGrid(bool all)
	{
		if (all)
		{
			clearAll();
			EntitiesPointers * entities_to_build = entities_container_->getAll();
			for (unsigned i = 0; i < entities_to_build->size(); ++i)
			{
				enqueue((*entities_to_build)[i]);
			}
		}
		else
		{
			clearDynamics();
			DynamicEntitiesPointers * entities_to_build = entities_container_->getDynamicEntities();
			for (unsigned i = 0; i < entities_to_build->size(); ++i)
			{
				enqueue((*entities_to_build)[i]);
			}
		}
	}

	void WorldGrid::clearAll()
	{
		for (WorldGridMatrix::iterator row = world_grid_.begin(); row != world_grid_.end(); ++row)
		{
			for (WorldGridRow::iterator col = row->begin(); col != row->end(); ++col)
			{
				(*col).clearAll();
			}
		}
		big_statics_.clear();
	}

	void WorldGrid::clearDynamics()
	{
		for (WorldGridMatrix::iterator row = world_grid_.begin(); row != world_grid_.end(); ++row)
		{
			for (WorldGridRow::iterator col = row->begin(); col != row->end(); ++col)
			{
				(*col).clearDynamics();
			}
		}
	}

	void WorldGrid::clearStatics()
	{
		for (WorldGridMatrix::iterator row = world_grid_.begin(); row != world_grid_.end(); ++row)
		{
			for (WorldGridRow::iterator col = row->begin(); col != row->end(); ++col)
			{
				(*col).clearStatics();
			}
		}
		big_statics_.clear();
	}

	void WorldGrid::enqueue(Entity * entity)
	{
		Eigen::Vector2d center = entity->getBoundingCylinder().getCenter();
		double radius = entity->getBoundingCylinder().getRadius();
		int row = (int) (floor(center[0] / cell_size_));
		int col = (int) (floor(center[1] / cell_size_));
		// The big ones.
		if (radius >= cell_size_ / 2)
		{
			// If not we've got a problem.
			assert(entity->isStatic());

			big_statics_.push_back(entity);
		}
		else
		{
			addToCell(row, col, entity);

			// Cells are checked within radius of one cell.
			if (center[0] - row * cell_size_ <= radius)
			{
				addToCell(row - 1, col, entity);
			}

			if ((row + 1) * cell_size_ - center[0] <= radius)
			{
				addToCell(row + 1, col, entity);
			}

			if (center[1] - col * cell_size_ <= radius)
			{
				addToCell(row, col - 1, entity);
			}

			if ((col + 1) * cell_size_ - center[1] <= radius)
			{
				addToCell(row, col + 1, entity);
			}

			double radius_sqr = math_tools::sqr(radius);
			if (distanceSqr(center, row * cell_size_, col * cell_size_) < radius_sqr)
			{
				addToCell(row - 1, col - 1, entity);
			}

			if (distanceSqr(center, (row + 1) * cell_size_, col * cell_size_) < radius_sqr)
			{
				addToCell(row + 1, col - 1, entity);
			}

			if (distanceSqr(center, row * cell_size_, (col + 1) * cell_size_) < radius_sqr)
			{
				addToCell(row - 1, col + 1, entity);
			}

			if (distanceSqr(center, (row + 1) * cell_size_, (col + 1) * cell_size_) < radius_sqr)
			{
				addToCell(row + 1, col + 1, entity);
			}
		}
	}

	CollisionPairsQueue * WorldGrid::getCollisionPairs()
	{
		CollisionPairsQueue *collision_pair_list = new CollisionPairsQueue();
		CollisionPairsSet already_added;
		// Go trough all the cells.
		for (WorldGridMatrix::iterator row = world_grid_.begin(); row != world_grid_.end(); ++row)
		{
			for (WorldGridRow::iterator col = row->begin(); col != row->end(); ++col)
			{
				addCollisionPairsFromCell(&(*col), collision_pair_list, &already_added);
			}
		}

		// Go trough the big ones.
		DynamicEntitiesPointers *dynamics = entities_container_->getDynamicEntities();
		CollisionPair current;
		for (unsigned i = 0; i < dynamics->size(); ++i)
		{
			for (unsigned j = 0; j < big_statics_.size(); ++j)
			{
				addToQueue(&already_added, CollisionPair((*dynamics)[i], big_statics_[j]), collision_pair_list);
			}
		}

		return collision_pair_list;
	}

	void WorldGrid::addCollisionPairsFromCell(WorldCell *cell, CollisionPairsQueue *collision_pair_list, CollisionPairsSet *already_added)
	{
		EntitiesPointers *statics = cell->getStatics();
		DynamicEntitiesPointers *dynamics = cell->getDynamics();
		CollisionPair current;
		// Statics vs. dynamics.
		for (unsigned i = 0; i < dynamics->size(); ++i)
		{
			for (unsigned j = 0; j < statics->size(); ++j)
			{
				addToQueue(already_added, CollisionPair((*dynamics)[i], (*statics)[j]), collision_pair_list);
			}

		}

		// Dynamics vs. dynamics.
		for (unsigned i = 0; i < dynamics->size(); ++i)
		{
			for (unsigned j = i + 1; j < dynamics->size(); ++j)
			{
				addToQueue(already_added, CollisionPair((*dynamics)[i], (*dynamics)[j]), collision_pair_list);
			}
		}
	}
}
